/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package org.gunncs;

import java.awt.BasicStroke;
import java.awt.Color;
import java.awt.Graphics;
import java.awt.Graphics2D;

/**
 *
 * @author anand
 */
public class RangeFinderModel extends SensorModel{
    public static final int ID = 1;
    public static int size = RobotModel.SIZE/2;
    public RangeFinderModel(){
        state = new double[3];
        state[0] = 40.0;
        state[1] = 0.0;
        state[2] = 0.0;
    }
    
    public void drawAt(Graphics g, int x, int y, double theta){
        g.setColor(Color.BLACK);
        Graphics2D g2d = (Graphics2D) g;
        float color = 1;
        for(int i=0; i<history.size(); i++){
          g.setColor(new Color(0.f, 0.f, 0.f, color));
          g.drawRect((int) (x+Math.cos(Math.toRadians(theta))*history.get(i)[0]*5),
                             (int) (y+Math.sin(Math.toRadians(theta))*history.get(i)[0]*5),2,2);
          color-=.1;
          if(color<0){
            color = .1f;
          }
        }
        g.setColor(Color.BLACK);
        g2d.setStroke(new BasicStroke(size/5));
        g.drawLine(x, y,(int) (x+Math.cos(Math.toRadians(theta))*size),
                        (int) (y+Math.sin(Math.toRadians(theta))*size));
        g2d.setStroke(new BasicStroke(2));
        g.setColor(Color.GREEN);
        g.drawLine(x, y,(int) (x+Math.cos(Math.toRadians(theta))*state[0]*5),
                        (int) (y+Math.sin(Math.toRadians(theta))*state[0]*5));
        g.setColor(Color.BLUE);
        g.drawString(""+state[0], (int) (x+Math.cos(Math.toRadians(theta))*state[0]*5),
                        (int) (y+Math.sin(Math.toRadians(theta))*state[0]*5));
        g.setColor(Color.RED);
        g.drawString(""+state[1], (int) (x+Math.cos(Math.toRadians(theta))*state[0]*5)+10,
                                        (int) (y+Math.sin(Math.toRadians(theta))*state[0]*5)+10);
    }


}
